#pragma once
point_2 get_point(graph::lat_lon_t lat, graph::lat_lon_t lon, graph::lat_lon_t center_lon)
{
    return point_2(graph::math::lat_lon_to_xy(lat, lon, center_lon).lat_, 
                   graph::math::lat_lon_to_xy(lat, lon, center_lon).lon_);
}
point_2 get_point(graph::vertex_t& v, graph::lat_lon_t center_lon)
{
    return get_point(v.get_point().lat_, v.get_point().lon_, center_lon);
}
void draw_marker(viewer::viewer_dc& dc, point_2 center)
{
    dc.drawPixel(center, 6);
    //dc.drawRectangle(point_2(center.x-0.05f, center.y-0.05f), point_2(center.x+0.05f, center.y+0.05f), true);
}
void print_un_processed_adj_vertices(viewer::printer& prn, std::vector<vertex_debug_t>& vertices)
{
    std::vector<vertex_debug_t>::iterator it = vertices.begin();
    std::vector<vertex_debug_t>::iterator it_end = vertices.end();
    prn << " ";
    (prn) << "distances of adjacent";
    prn << "unprocessed vertices:";
    //printf("Adjacent unprocessed vertices: \n");
    for(;it != it_end; ++it)
    {
        //printf("Lat: %f \n Lon: %f \n Distance: %f \n Heuristic %f \n Potential: %f \n", 
        //    (*it).lat_, 
        //    (*it).lon_,
        //    (*it).distance_,
        //    (*it).heuristic_,
        //    (*it).distance_ + (*it).heuristic_);
        //printf("\n");
        (prn) << (*it).distance_;
    }
    prn << " ";
    prn << "--------------";
}

struct view_drawer_t
{
    view_drawer_t(viewer::viewer_dc& dc):
        dc_(dc),
        prev_path_type_(graph::PATH_TYPE_NONE)
    {}
    
    void draw_view(graph::point_t view_square_center, graph::lat_lon_t center_lon)
    {
        graph::global_data_t& global = graph::global::get_instance();
        if (prev_path_type_ != global.current_path_type_)
            cache_lines();
        lines_t::iterator it = lines_.begin();
        lines_t::iterator end = lines_.end();
        for(; it != end; ++it)
        {
           dc_.drawLine(
                    get_point((*it).first.lat_, (*it).first.lon_, center_lon),
                    get_point((*it).second.lat_, (*it).second.lon_, center_lon),
                    1
                );
        }
        
        //{
        //   dc_.setColor(colorf(1.0f, 1.0f, 0.0f));
        //   graph::details::graph_metadata_t const & metadata = *global.graph_->meta_data_vect_;
        //   graph::details::cell_edge_iterator_pair_t its = 
        //       metadata.get_cells<graph::details::d2>();
        //   for(; its.first != its.second; ++its.first)
        //   {
        //      graph::point_pair_t rect = 
        //       graph::details::graph_metadata_t::get_cell_rect<graph::details::d1>(its.first->first);
        //      draw_square(rect, center_lon);
        //   }
        //}
        
        dc_.setColor(colorf(1.0f, 1.0f, 0.0f));
        graph::details::graph_metadata_t const & metadata = global.graph_->get_metadata();
        graph::details::cell_edge_iterator_pair_t its = 
            metadata.get_cells<graph::details::d1>();
        //for(; its.first != its.second; ++its.first)
        //{
        //   graph::point_pair_t rect = 
        //    graph::details::graph_metadata_t::get_cell_rect<graph::details::d1>(its.first->first);
        //   draw_square(rect, center_lon);
        //}
        
        dc_.setColor(colorf(0.5f, 1.0f, 0.0f));
        its = metadata.get_cells<graph::details::d2>();
        for(; its.first != its.second; ++its.first)
        {
           graph::point_pair_t rect = 
            graph::details::graph_metadata_t::get_cell_rect<graph::details::d2>(its.first->first); 
           draw_square(rect, center_lon);
        }
        
        /*dc_.setColor(colorf(1.0f, 0.5f, 0.5f));
        its = metadata.get_cells<graph::details::d4>();
        for(; its.first != its.second; ++its.first)
        {
           graph::point_pair_t rect = 
            graph::details::graph_metadata_t::get_cell_rect<graph::details::d4>(its.first->first); 
           draw_square(rect, center_lon);
        }*/
    }
private:
    typedef std::vector<graph::point_pair_t> lines_t;
    lines_t lines_;
    std::string prev_path_type_;
    viewer::viewer_dc& dc_;
    
    void cache_lines()
    {
      graph::global_data_t& global = graph::global::get_instance();
      lines_.clear();
      prev_path_type_ = global.current_path_type_;
      //cache graph lines here
      graph::view_vertex_iterator_pair_t vert_it = graph::vertices(*global.current_view_);            
      for(; vert_it.first != vert_it.second; ++vert_it.first)
      {
         graph::view_adj_vertices_iterator_pair_t<graph::reverse_adj>::type 
            adj_vert_it = graph::adj_vertices<graph::reverse_adj>(
               (*vert_it.first).get_id() ,*global.current_view_
            );
         for(; adj_vert_it.first != adj_vert_it.second; ++adj_vert_it.first)
         {
            lines_.push_back(graph::point_pair_t(
                  (*vert_it.first).get_point(),
                  (*adj_vert_it.first).get_point()
               )
            );
         }
      }
    }
    
    void draw_square(graph::point_pair_t pp, 
                     graph::lat_lon_t center_lon)
    {
        point_2 p11 = get_point(pp.first.lat_, pp.first.lon_, center_lon);
        point_2 p12 = get_point(pp.first.lat_ + abs(pp.second.lat_- pp.first.lat_), pp.first.lon_, center_lon);
        point_2 p21 = get_point(pp.first.lat_, pp.first.lon_ + abs(pp.second.lon_ - pp.first.lon_), center_lon);
        point_2 p22 = get_point(pp.first.lat_ + abs(pp.second.lat_- pp.first.lat_), pp.first.lon_ + abs(pp.second.lon_ - pp.first.lon_), center_lon);
        //
        //p11.x += foo;
        //p11.y -= foo;
        //p12.x += foo;
        //p12.y += foo;
        //p21.x -= foo;
        //p21.y -= foo;
        //p22.x -= foo;
        //p22.y += foo;
        
        dc_.drawLine(p11, p12);
        dc_.drawLine(p21, p22);
        dc_.drawLine(p22, p12);
        dc_.drawLine(p11, p21);
            
        /*dc_.drawLine(
                get_point(pp.first.lat_, pp.first.lon_, center_lon),
                get_point(pp.second.lat_, pp.second.lon_, center_lon)
            );
        dc_.drawLine(
                get_point(pp.first.lat_, pp.second.lon_, center_lon),
                get_point(pp.second.lat_, pp.first.lon_, center_lon)
            );*/
    }
};


//void draw_astar_sh_path(viewer::viewer_dc& dc, astar_search_t<debug, undirectional>& as, graph::lat_lon_t center_lon)
//{
//    graph::global_data_t& global = graph::global::get_instance();
//    dc.setColor(colorf(1.0f, 0.7f, 0.85f));
//    graph::vertex_id_t finish_id = as.get_finish_vertex_id();
//    graph::vertex_id_t start_id = as.get_start_vertex_id();
//    graph::vertex_id_t temp_id = as.get_finish_vertex_id();
//    graph::vertex_id_t temp_prev_id = as.get_finish_vertex_id();
//    
//    vertex_data_container_t<astar_vertex_data_t>& vert_data = as.get_vertex_data_fwd();
//    while (temp_id != start_id)
//    {
//        graph::vertex_t& temp_v = global.graph_->get_vertex_by_id(temp_id);
//        graph::vertex_t& temp_prev_v = global.graph_->get_vertex_by_id(temp_prev_id);
//        dc.drawLine(get_point(temp_v, center_lon), get_point(temp_prev_v.get_point().lat_, temp_prev_v.get_point().lon_, center_lon) );
//        temp_prev_id = temp_id;
//        temp_id = vert_data.get_value(temp_id).predecessor_;
//    }
//    dc.drawLine(
//        get_point( global.graph_->get_vertex_by_id(temp_prev_id) , center_lon), 
//        get_point( global.graph_->get_vertex_by_id(temp_id), center_lon) 
//        );   
//}
template<directional_e direction>
void draw_astar_sh_path(viewer::viewer_dc& dc, astar_search_t<debug, direction>& as, graph::lat_lon_t center_lon)
{
   static std::list<graph::lat_lon_t> sh_path;
   static graph::point_pair_t prev_vert_points;
   graph::point_pair_t current_vert_points(
      graph::point_t(as.get_start_lat(), as.get_start_lon()), 
      graph::point_t(as.get_finish_lat(), as.get_finish_lon())
   );
   if (prev_vert_points.first.lat_ != current_vert_points.first.lat_ || 
       prev_vert_points.first.lon_ != current_vert_points.first.lon_ || 
       prev_vert_points.second.lat_ != current_vert_points.second.lat_ || 
       prev_vert_points.second.lon_ != current_vert_points.second.lon_ )
   {   
      sh_path.clear();
      as.fill_shpath(sh_path);
   }
   std::list<graph::lat_lon_t>::iterator it = sh_path.begin();
   std::list<graph::lat_lon_t>::iterator it_end = sh_path.end();
   std::advance(it_end, -2);
   dc.setColor(colorf(0.0f, 0.0f, 1.0f));
   while(it != it_end)
   {
      graph::point_t temp_p1;
      temp_p1.lat_ = *it;
      temp_p1.lon_ = *(++it);
      graph::point_t temp_p2;
      temp_p2.lat_ = *(++it);
      std::list<graph::lat_lon_t>::iterator it_temp(it);
      temp_p2.lon_ = *(++it_temp);
      dc.drawLine(
         get_point(temp_p1.lat_, temp_p1.lon_, center_lon), 
         get_point(temp_p2.lat_, temp_p2.lon_, center_lon) );
   }
   prev_vert_points = current_vert_points;
}
void draw_astar(viewer::viewer_dc& dc, astar_search_t<debug, undirectional>& as, graph::lat_lon_t center_lon)
{
    graph::global_data_t& global = graph::global::get_instance();

    dc.setColor(colorf(0.7f, 0.7f, 0.7f)); // grey
    draw_marker(dc, get_point(as.get_start_lat(), as.get_start_lon(), center_lon));
    draw_marker(dc, get_point(as.get_finish_lat(), as.get_finish_lon(), center_lon));

    dc.setColor(colorf(0.7f, 0.7f, 0.7f)); //grey
    draw_marker(dc, get_point(as.debug_info_->deb_vertex_prev1_.lat_, 
                              as.debug_info_->deb_vertex_prev1_.lon_, center_lon));
    dc.setColor(colorf(1.0f, 0.8f, 0.0f)); //orange 
    draw_marker(dc, get_point(as.debug_info_->deb_vertex_current1_.lat_, 
                              as.debug_info_->deb_vertex_current1_.lon_, center_lon));    

    viewer::printer prn(dc, point_2i(10, 50));
    dc.setTextColor(colorf(1.0f, 0.8f, 0.0f)); //orange 
    prn << " ";
    prn << "Summary: ";
    prn << "--------------";
    prn << " ";
    prn << "Iteration: ";
    prn << as.debug_info_->num_iterates_;
    prn << "visited_vertices: ";
    prn << as.debug_info_->visited_vertices_;
    prn << "visited edges: ";
    prn << as.debug_info_->visited_edges_;
    prn << " ";
    prn << "--------------";
    prn << " ";
    dc.setTextColor(colorf(0.7f, 0.7f, 0.7f)); //grey
    prn << "Previous vertex:";
    prn << "Distance:";
    prn << as.debug_info_->deb_vertex_prev1_.distance_;
    prn << "Potential:";
    prn << as.get_potential<forward>(graph::point_t(as.debug_info_->deb_vertex_prev1_.lat_, 
                                                    as.debug_info_->deb_vertex_prev1_.lon_));
    //prn << "Dist + Potential:";
    //prn << as.deb_vertex_prev1_.distance_ + as.deb_vertex_prev1_.heuristic_;
    print_un_processed_adj_vertices(prn, as.debug_info_->deb_vertex_prev1_adj);

    dc.setTextColor(colorf(1.0f, 0.8f, 0.0f)); //orange 
    prn << "Current vertex:";
    prn << "Distance:";
    prn << as.debug_info_->deb_vertex_current1_.distance_;
    //prn << "Heuristic:";
    //prn << as.deb_vertex_current1_.heuristic_;
    prn << "Potential:";
    prn << as.get_potential<forward>(graph::point_t(as.debug_info_->deb_vertex_current1_.lat_,
                                                    as.debug_info_->deb_vertex_current1_.lat_));

    dc.setTextColor(colorf(1.0f, 1.0f, 1.0f)); //white 
    prn << "Sub routine calls:";
    prn << "Opened list 1 change priority:";
    prn << as.debug_info_->min_queue_fwd_chpriority_calls_;
    prn << "Opened list 1 extract min:";
    prn << as.debug_info_->min_queue_fwd_extract_min_fwd_calls_;
    prn << "Opened list 1 is in:";
    prn << as.debug_info_->min_queue_fwd_isin_calls_;
    prn << "Opened list 1 max elems:";
    prn << as.debug_info_->min_queue_fwd_max_elems_;
    prn << "Opened list 1 put:";
    prn << as.debug_info_->min_queue_fwd_put_calls_;
    prn << "Closed list 1 is in:";
    prn << as.debug_info_->closed_list1_isin_calls_;
    prn << "Closed list 1 put:";
    prn << as.debug_info_->closed_list1_put_calls_;
    prn << "Put predecessor 1:";
    prn << as.debug_info_->set_predecessor_fwd_calls_;
    prn << "Get distance 1:";
    prn << as.debug_info_->get_distance_fwd_calls_;
    
    // if astar ended, draw shortest path
    if (as.is_all_ended())
        draw_astar_sh_path(dc, as, center_lon);

}
void draw_astar(viewer::viewer_dc& dc, astar_search_t<debug, bidirectional>& as, graph::lat_lon_t center_lon)
{
    graph::global_data_t& global = graph::global::get_instance();
    
    dc.setColor(colorf(0.7f, 0.7f, 0.7f)); // grey
    draw_marker(dc, get_point(as.get_start_lat(), as.get_start_lon(), center_lon));
    draw_marker(dc, get_point(as.get_finish_lat(), as.get_finish_lon(), center_lon));

    //showing visited vertices
    dc.setColor(colorf(1.0f, 0.8f, 0.0f)); // orange
    output_list_fwd_t& o_list = as.debug_info_->output_list_fwd_;
    output_list_fwd_t::iterator it = o_list.begin();
    output_list_fwd_t::iterator it_end = o_list.end();
    line_t back;
    for(; it!=it_end; ++it)
    {
        dc.drawLine(
            get_point((*it).first.lat_, (*it).first.lon_, center_lon),
            get_point((*it).second.lat_,(*it).second.lon_, center_lon),
            1
            );
        back = (*it);
    }
    
    //showing visited vertices
    dc.setColor(colorf(0.0f, 0.8f, 1.0f)); //light blue
    output_list_fwd_t& o_list2 = as.debug_info_->output_list_bkwd_;
    it = o_list2.begin();
    it_end = o_list2.end();
    for(; it!=it_end; ++it)
    {
        dc.drawLine(
            get_point((*it).first.lat_, (*it).first.lon_, center_lon),
            get_point((*it).second.lat_,(*it).second.lon_, center_lon),
            1
            );
        back = (*it);
    }
    // first direction of search and summary
    viewer::printer prn(dc, point_2i(10, 50));
    dc.setTextColor(colorf(1.0f, 0.8f, 0.0f)); //orange 
    prn << " ";
    prn << "Summary: ";
    prn << "--------------";
    prn << " ";
    prn << "Iteration: ";
    prn << as.debug_info_->num_iterates_;
    prn << "visited_vertices: ";
    prn << as.debug_info_->visited_vertices_;
    prn << "visited edges: ";
    prn << as.debug_info_->visited_edges_;
    prn << " ";
    prn << "--------------";
    prn << " ";
    dc.setTextColor(colorf(0.7f, 0.7f, 0.7f)); //grey
    prn << "Previous vertex:";
    prn << "Distance:";
    prn << as.debug_info_->deb_vertex_prev1_.distance_;
    prn << "Potential:";
    prn << as.get_potential<forward>(graph::point_t(as.debug_info_->deb_vertex_prev1_.lat_, 
                                                    as.debug_info_->deb_vertex_prev1_.lon_));
    //prn << "Dist + potential:";
    //prn << as.deb_vertex_prev1_.distance_ + as.deb_vertex_prev1_.heuristic_;
    print_un_processed_adj_vertices(prn, as.debug_info_->deb_vertex_prev1_adj);

    dc.setTextColor(colorf(1.0f, 0.8f, 0.0f)); //orange 
    prn << "Current vertex:";
    prn << "Distance:";
    prn << as.debug_info_->deb_vertex_current1_.distance_;
    prn << "Potential:";
    prn << as.get_potential<forward>(graph::point_t(as.debug_info_->deb_vertex_current1_.lat_,
                                                    as.debug_info_->deb_vertex_current1_.lon_));
    draw_marker(dc, get_point(as.debug_info_->deb_vertex_current1_.lat_, 
                              as.debug_info_->deb_vertex_current1_.lon_, center_lon));
    //prn << "Dist + potential:";
    //prn << as.deb_vertex_current1_.distance_ + as.deb_vertex_current1_.heuristic_;

    dc.setTextColor(colorf(1.0f, 1.0f, 1.0f)); //white 
    prn << "Sub routine calls:";
    prn << "Opened list 1 change priority:";
    prn << as.debug_info_->min_queue_fwd_chpriority_calls_;
    prn << "Opened list 1 extract min:";
    prn << as.debug_info_->min_queue_fwd_extract_min_fwd_calls_;
    prn << "Opened list 1 is in:";
    prn << as.debug_info_->min_queue_fwd_isin_calls_;
    prn << "Opened list 1 max elems:";
    prn << as.debug_info_->min_queue_fwd_max_elems_;
    prn << "Opened list 1 put:";
    prn << as.debug_info_->min_queue_fwd_put_calls_;
    prn << "Closed list 1 is in:";
    prn << as.debug_info_->closed_list1_isin_calls_;
    prn << "Closed list 1 put:";
    prn << as.debug_info_->closed_list1_put_calls_;
    prn << "Put predecessor 1:";
    prn << as.debug_info_->set_predecessor_fwd_calls_;
    prn << "Get distance 1:";
    prn << as.debug_info_->get_distance_fwd_calls_;

    // second direction of search
    viewer::printer prn2(dc, point_2i(1000, 50));
    dc.setTextColor(colorf(0.0f, 0.8f, 1.0f)); //orange
    prn2 << " ";
    prn2 << "Summary: ";
    prn2 << "--------------";
    prn2 << " ";
    prn2 << "Iteration: ";
    prn2 << as.debug_info_->num_iterates2_;
    prn2 << "visited_vertices: ";
    prn2 << as.debug_info_->visited_vertices2_;
    prn2 << "visited edges: ";
    prn2 << as.debug_info_->visited_edges2_;
    prn2 << " ";
    prn2 << "--------------";
    prn2 << " ";
    dc.setTextColor(colorf(0.7f, 0.7f, 0.7f)); //grey
    prn2 << "Previous vertex:";
    prn2 << "lat:";
    prn2 << as.debug_info_->deb_vertex_prev2_.lat_;
    prn2 << "lon:";
    prn2 << as.debug_info_->deb_vertex_prev2_.lon_;
    prn2 << "Distance:";
    prn2 << as.debug_info_->deb_vertex_prev2_.distance_;
    //prn2 << "Heuristic:";
    //prn2 << as.deb_vertex_prev2_.heuristic_;
    prn2 << "Potential:";
    prn2 << as.get_potential<backward>(graph::point_t(as.debug_info_->deb_vertex_prev2_.lat_,
                                                      as.debug_info_->deb_vertex_prev2_.lon_));
    print_un_processed_adj_vertices(prn2, as.debug_info_->deb_vertex_prev2_adj);

    dc.setTextColor(colorf(0.0f, 0.8f, 1.0f)); //orange
    prn2 << "Current vertex:";
    prn2 << "lat:";
    prn2 << as.debug_info_->deb_vertex_current2_.lat_;
    prn2 << "lon:";
    prn2 << as.debug_info_->deb_vertex_current2_.lon_;
    prn2 << "Distance:";
    prn2 << as.debug_info_->deb_vertex_current2_.distance_;
    //prn2 << "Heuristic:";
    //prn2 << as.deb_vertex_current2_.heuristic_;
    prn2 << "Potential:";
    prn2 << as.get_potential<backward>(graph::point_t(as.debug_info_->deb_vertex_current2_.lat_,
                                                      as.debug_info_->deb_vertex_current2_.lon_));
    draw_marker(dc, get_point(as.debug_info_->deb_vertex_current2_.lat_, 
                                 as.debug_info_->deb_vertex_current2_.lon_, center_lon));
    
    // if astar ended, draw shortest path
    if (as.is_all_ended())
        draw_astar_sh_path(dc, as, center_lon);
}

graph::vertex_id_t get_nearest_vertex_id(graph::lat_lon_t global_x, graph::lat_lon_t global_y, graph::lat_lon_t center_lon)
{
    graph::global_data_t& global = graph::global::get_instance();

    graph::point_t p (
      graph::math::xy_to_lat_lon(global_x, global_y, center_lon).lat_,
      graph::math::xy_to_lat_lon(global_x, global_y, center_lon).lon_
    );
            
    graph::vertex_id_t id;
    std::vector<graph::edge_t> nearest_edges;
    graph::point_t nearest;
    global.current_view_->get_nearest_vertex(p, id);
    /*if (!global.current_view_->get_nearest_edges(p, nearest_edges))
      return 0;
    id = nearest_edges.front().get_target_vertex_id();      */
    return id;
}

//now we print it to console
template<directional_e direction>
void print_vertex_info(graph::vertex_id_t id, astar_search_t<debug, direction>& as, viewer::viewer_dc& dc, graph::lat_lon_t center_lon)
{
    graph::global_data_t& global = graph::global::get_instance();
    dc.setColor(colorf(0.0f,0.0f,1.0f));
    vertex_debug_t v = as.get_debug_vertex_by_id(id, true);
    draw_marker(dc, get_point(v.lat_, v.lon_, center_lon));
    
    viewer::printer prn(dc, point_2i(1000, dc.screenViewport()[1].hi() - 140));
    dc.setTextColor(colorf(1.0f, 1.0f, 1.0f)); //white
    prn << "Latitude: ";
    prn << v.lat_;
    prn << "Longtitude: ";
    prn << v.lon_;
    prn << "Distance: ";
    prn << v.distance_;
    prn << "Potential: ";
    prn << as.get_potential<forward>(graph::point_t(v.lat_, v.lon_));
    //prn << "Potential+Distance: ";
    //prn << v.heuristic_ + v.distance_;
    if(direction == bidirectional)
    {
        viewer::printer prn(dc, point_2i(800, dc.screenViewport()[1].hi() - 155));
        dc.setTextColor(colorf(1.0f, 1.0f, 1.0f)); //white
        v = as.get_debug_vertex_by_id(id, false);
        prn << "Second dimension: ";
        prn << "Latitude: ";
        prn << v.lat_;
        prn << "Longtitude: ";
        prn << v.lon_;
        prn << "Distance: ";
        prn << v.distance_;
        prn << "Potential: ";
        prn << as.get_potential<backward>(graph::point_t(v.lat_, v.lon_));
        //prn << "Potential+Distance: ";
        //prn << v.heuristic_ + v.distance_;
    }
}